/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#define MAP_FILE_NAME "../../res/autmap-strip.txt"

#define PROCESS_MCL

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG
#include "CTopoOccuMCL.h"
#include "CTopoScanOccuMCL.h"

#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch 
#include "update_pose.h"

#define LASER_FILE "laser_update.txt" ///< laser file and silmupose and laser pose
#define SEPI_LASER_FILE "sepi_lodo.txt"
///This is the topo+batch
//#define COR_PATH_FILE "topo_odo.txt"  ///< "Correct path"
///This is map correlated
#define COR_PATH_FILE "pose_measurement.txt"  ///< "Correct path"


#define TOPO_FILE "topo_path.txt"     ///< result of the topo MCL

#define NUM_OF_MCL_RUNS 7

static char color_r[NUM_OF_MCL_RUNS]={0  ,0  ,0  ,0  ,128,255,255};
static char color_g[NUM_OF_MCL_RUNS]={128,255,0  ,0  ,0  ,0  ,0};
static char color_b[NUM_OF_MCL_RUNS]={0  ,0  ,128,255,0  ,0  ,255};

TOccupancyGrid *O,*O2;

vector<mcl::pose> cor_pose;
vector<mcl::pose> topo_path;
vector<mcl::pose> topo_cov;
vector<mcl::pose> silmu;
vector<mcl::pose> laser;		
vector<mcl::pose> sepi_lodo;

		
float topi(float da){
	if(da>M_PI)while(da>M_PI) da-=2.0*M_PI;
	else if(da<-M_PI)while(da<-M_PI) da+=2.0*M_PI;
	return da;
}


int readLaserOdometryData(FILE *f,mcl::scan &meas, mcl::pose &lp, mcl::pose &sp){
	float p1,p2,p3;
	int i;
	char c;
	fscanf(f,"%f %f %f %f %f %f ", &lp.x,&lp.y,&lp.a,&sp.x,&sp.y,&sp.a); ///< Pose
	for(i=0;i<361;i++){
		fscanf(f,"%f",&meas.r[i]);
		meas.a[i]=0.5*i*M_PI/180.0;
		fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
		if(c=='\n' && i<361){
			fprintf(stderr,"EOL reached at i=%d\n",i);
			meas.N = i;
			return 0;
		}
	}
	meas.N = 361;
	return 0;
}

int readCorrectPose(FILE *f, mcl::pose &p){
	fscanf(f,"%f %f %f\n",&p.x,&p.y,&p.a);
	return 0;
}
		
int readTopoPose(FILE *f, mcl::pose &p, mcl::pose &var){
	fscanf(f,"%f %f %f %f %f %f\n",&p.x,&p.y,&p.a,&var.x,&var.y,&var.a);
  return 0;
}		

void readMCLfile(const char *fname, vector< vector<mcl::pose> >	&pose_storage){
	
	FILE *f = fopen(fname,"rt");
	if(f == NULL){
		fprintf(stderr,"readMCLfile:: Unable to open '%s'\n",fname);
		exit(1);
	}
	
	vector<mcl::pose> tmp;
	for(int i=0;i<NUM_OF_MCL_RUNS;i++){
		pose_storage.push_back(tmp);
	}
		
	
	mcl::pose p;
	
	while(!feof(f)){
		
		for(int i=0;i<NUM_OF_MCL_RUNS-1;i++){
			fscanf(f,"%f %f %f ",&p.x,&p.y,&p.a);
			pose_storage[i].push_back(p);
		}
		fscanf(f,"%f %f %f\n",&p.x,&p.y,&p.a);
		pose_storage[NUM_OF_MCL_RUNS-1].push_back(p);
	}
	
	fprintf(stderr,"We have %d poses in %d vectors\n",pose_storage[0].size(),pose_storage.size());
	
	fclose(f);
}		
							
float computePathLength(vector<mcl::pose> &path){
	float l=0;
	float dx,dy;
	for(int i=0;i<path.size()-1;i++){
		dx=path[i].x - path[i+1].x;
		dy=path[i].y - path[i+1].y;
		l+=sqrt(dx*dx+dy*dy);
	}
	return l;
}
		
mcl::pose mean(vector<mcl::pose> &p){
	mcl::pose avg(0,0,0);
	if(p.size()==0) return avg;
	
	for(int i=0;i<p.size();i++){
		avg.x +=p[i].x;
		avg.y +=p[i].y;
		avg.a +=topi(p[i].a);
	}
	avg.x /=p.size();
	avg.y /=p.size();
	avg.a /=p.size();

	return avg;
}
		
mcl::pose min(vector<mcl::pose> &p){
	mcl::pose min(100000,100000,100000);
	
	for(int i=0;i<p.size();i++){
		if(p[i].x < min.x) min.x = p[i].x;
		if(p[i].y < min.y) min.y = p[i].y;
		if(topi(p[i].a) < min.a) min.a = topi(p[i].a);
	}
	
	return min;
}

mcl::pose max(vector<mcl::pose> &p){
	mcl::pose min(-100000,-100000,-100000);
	
	for(int i=0;i<p.size();i++){
		if(p[i].x > min.x) min.x = p[i].x;
		if(p[i].y > min.y) min.y = p[i].y;
		if(topi(p[i].a) > min.a) min.a = topi(p[i].a);
	}
	
	return min;
}

void diff(vector<mcl::pose> &p1,vector<mcl::pose> &p2,vector<mcl::pose> &dif){
	mcl::pose p;
	for(int i=0;i<p1.size();i++){
		p.set(p1[i].x-p2[i].x,p1[i].y-p2[i].y,topi(p1[i].a-p2[i].a));
		dif.push_back(p);
	}
}		
float computeEndAngleError(vector<mcl::pose> &real, vector<mcl::pose> &est){
		float da=0;
		int N=0;
		/*
		for(int i=0;i<30;i++){
				
				fprintf(stderr,"%.3f %.3f %.3f\n",real[real.size()-1-i].a , est[est.size()-1-i].a, 
								topi(real[real.size()-1-i].a - est[est.size()-1-i].a));
				//da+=real[real.size()-1-i].a - est[est.size()-1-i].a;
				//N++;
		}
		*/
		da = (real[real.size()-1].a - est[est.size()-1].a);
		
		if(da>M_PI)while(da>M_PI) da-=2.0*M_PI;
		else if(da<-M_PI)while(da<-M_PI) da+=2.0*M_PI;
		//fprintf(stderr,"da=%.3f\n",da);
		return da;
}
		

vector<mcl::pose> godo; ///< Will be the corrected odometry
vector<mcl::pose> odo;  ///< Original odometry
static int lastCorrected=-1;
void odoCorrector(mcl::pose &p, int index){
		mcl::pose mest;
		mcl::pose corPos;
		
		if(lastCorrected == -1){
				lastCorrected = index;
				godo[index]=p;
				for(int i=index;i<godo.size()-1;i++){
						mest.setToDifferentialPose( odo[i+1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i+1] = corPos;
				}
				for(int i=index;i>0;i--){
						mest.setToDifferentialPose( odo[i-1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i-1] = corPos;
				}	
		}
		else{
				mcl::pose orig = godo[index];
				mcl::pose err = orig;
				int N = index-lastCorrected;
				int last = lastCorrected;
						
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
				
				if(err.a > M_PI){
						
						//fprintf(stderr,"WARNING: Not good angle error!\n");
						while(err.a>M_PI) err.a-=2.0*M_PI; 
				}else if(err.a < -M_PI){
						//fprintf(stderr,"WARNING: Not good angle error!\n");
						while(err.a < -M_PI) err.a+=2.0*M_PI; 
				}
				
				fprintf(stderr,"Orig_err (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				err.a/=(float)(N);
				
				
				lastCorrected = index;
				godo[index]=p;
				for(int i=index;i<godo.size()-1;i++){
						mest.setToDifferentialPose( odo[i+1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i+1] = corPos;
				}
				///Now compute the correction from the last correctio to present
				/// First the heading
				godo[index] = orig;
				for(int i=last;i<index;i++){
						mest.setToDifferentialPose( godo[i+1],godo[i]);
						if(i>last) godo[i] = corPos; 
						mest.a -= err.a;
						corPos = godo[i].integrateDifferential(mest);
				}
				godo[index]=corPos;
			//	mest.setToDifferentialPose( godo[i+1],godo[i]);
			//	mest.a += err.a;
			//	corPos = godo[i].integrateDifferential(mest);
				
				
				
				///////// Virhe ....
				err = godo[index];
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
				//fprintf(stderr,"Err_Now (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				err.y/=(float)(N);
				err.x/=(float)(N);
				
				for(int i=last;i<=index;i++){
						godo[i].x -= (i-last)*err.x;
						godo[i].y -= (i-last)*err.y;
				}
				err = godo[index];
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
			//	fprintf(stderr,"Final (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				
		}
}


//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
		rgb[3*Nx*j+3*i]=r;
		rgb[3*Nx*j+3*i+1]=g;
		rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
		float *data = gr->getDataPtr();
		int Nx,Ny;
		gr->getDim(Nx,Ny);
		float v;
	
		for(int j=0;j<Ny;j++){
				for(int i=0;i<Nx;i++){
						v = data[Nx*j+i];
				//if(v>0.29) v = 0.9;
						setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
				}
		}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											RGB[3*Nx*y+3*x] = r;
											RGB[3*Nx*y+3*x+1] = g;
											RGB[3*Nx*y+3*x+2] = b;
											i++;
											rr+=resolution;
									}
}

void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////


int main(int argc, char *argv[]){
	mcl::scan s; ///< for occupancy grid
	s.allocate(361);
	Grid::TCoord2D ocoord;
	Grid::TPixel pix;
	mcl::pose p1,p2;	
	
if(argc != 2){
			fprintf(stderr,"\nUsage: ./map_matcher [dir]\n\n");
			fprintf(stderr,"[dir] - the directory where the laser_update.txt is located\n");
			return 0;
		}
		
		O = new TOccupancyGrid(1,1,0.1,Grid::TCoord2D(0,0));
		if(!O->loadLineMap(MAP_FILE_NAME, 1.0,1.0)){
				fprintf(stderr,"**** Unable to load the map\n");
				exit(1);
		}
		
		O2 = new TOccupancyGrid(100,40,0.1,Grid::TCoord2D(0,0));
		O2->setGridToValue(1.0);
		
		char *directory=argv[1];
		fprintf(stderr,"Using directory %s\n",directory);

		if(chdir(directory)==-1){
			fprintf(stderr,"Invalid directory '%s'!\n",directory);
			return 0;
		
		}
		FILE *flaser = fopen(LASER_FILE,"rt");
		if(flaser==NULL){
			fprintf(stderr,"No such file\n");	
			return 0;
		}
		
		FILE *fcor = fopen(COR_PATH_FILE,"rt");
		if(fcor==NULL){
			fprintf(stderr,"No such file\n");	
			return 0;
		}
		
		FILE *ftopo = fopen(TOPO_FILE,"rt");
		if(ftopo==NULL){
			fprintf(stderr,"No such file\n");	
			return 0;
		}
		
		FILE *fsepi = fopen(SEPI_LASER_FILE,"rt");
		if(fsepi==NULL){
			fprintf(stderr,"No such file '%s' \n",SEPI_LASER_FILE);	
			return 0;
		}
		
		int Nx,Ny,Nx2,Ny2;
		O->getDim(Nx,Ny);
		O2->getDim(Nx2,Ny2);
		unsigned char *rgb = (unsigned char *) malloc(3*Nx*Ny);
		unsigned char *rgb2 = (unsigned char *) malloc(3*Nx2*Ny2);
		addImageWindow(Nx, Ny);
		//addImageWindow(Nx2, Ny2);
		glutImageGUI_Start();
		
		
		while(!feof(flaser) && !feof(fcor) && !feof(ftopo) && !feof(fsepi)){
			readLaserOdometryData(flaser,s,p1,p2);
			silmu.push_back(p2);
			laser.push_back(p1);
			//fprintf(stderr,"Laser=(%f,%f,%f) ",p1.x,p1.y,p1.a);
			
			readCorrectPose(fcor,p1);
			cor_pose.push_back(p1);
			//fprintf(stderr,"COR=(%f,%f,%f) ",p1.x,p1.y,p1.a);
			
			readTopoPose(ftopo,p1,p2);
			topo_path.push_back(p1);
			topo_cov.push_back(p2);
			//fprintf(stderr,"Topo=(%f,%f,%f) \n",p1.x,p1.y,p1.a);
			
			readCorrectPose(fsepi,p1);
			sepi_lodo.push_back(p1);
			////////////////////////////////////////////////////////////////////
		}
		
		
		odo=silmu;
		godo = odo;
		odoCorrector(cor_pose[0], 0);
		silmu=godo;
		
		odo=laser;
		godo = odo;
		odoCorrector(cor_pose[0], 0);
		laser=godo;
		
		odo=sepi_lodo;
		godo = odo;
		odoCorrector(cor_pose[0], 0);
		sepi_lodo=godo;
		
		int cnt = 0;
		
		setRGBmap(rgb,O, 200,220, 200);
		cnt = 0;
		displayImage(0, rgb,Ny,Nx);
		fprintf(stderr,"HIT ENTER TO START\n");
		char charachter;
		fscanf(stdin,"%c",&charachter);
		
		while(cnt<cor_pose.size()){
				//usleep(1000*1);
					
				//ocoord.x = cor_pose[cnt].x;ocoord.y = cor_pose[cnt].y;
				//pix = O->toPixel(ocoord);
				//setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
				
				//ocoord.x = silmu[cnt].x;ocoord.y = silmu[cnt].y;
				//pix = O->toPixel(ocoord);
				//setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,255, 0 );
				
				ocoord.x = laser[cnt].x;ocoord.y = laser[cnt].y;
				pix = O->toPixel(ocoord);
				setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,255, 255 );
				
				//ocoord.x = sepi_lodo[cnt].x;ocoord.y = sepi_lodo[cnt].y;
				//pix = O->toPixel(ocoord);
				//setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,255, 255 );
				
				cnt +=1;

				if(cnt%100==0)displayImage(0, rgb,Ny,Nx);
		}
		
		saveJPEG("odo_result.jpg",rgb,Nx,Ny);
		//setRGBmap(rgb,O, 200,220, 200);
		//displayImage(0, rgb,Ny,Nx);
		fprintf(stderr,"****** DONE ****** \n HIT ENTER TO CONTINUE\n");
		fscanf(stdin,"%c",&charachter);
#ifdef PROCESS_MCL
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		vector< vector<mcl::pose> >	pose_storage;
		fprintf(stderr,"#################### TOPO ###############################\n");
		readMCLfile("topoMCL_pose.txt",pose_storage);
				
		setRGBmap(rgb,O, 200,220, 200);
		cnt=0;
		while(cnt<pose_storage[0].size()){
			for(int j = 0; j<NUM_OF_MCL_RUNS;j++){
					ocoord.x = pose_storage[j][cnt].x;ocoord.y = pose_storage[j][cnt].y;
					pix = O->toPixel(ocoord);
					//setRGBpixBold(rgb,pix.x,pix.y, Nx, color_r[j],color_g[j], color_b[j] );
					setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,115+20*j, 0 );
			}
			cnt++;
			usleep(1000*1);
			if(cnt%20==0) displayImage(0, rgb,Ny,Nx);
		}
		
		//saveJPEG("topo_result.jpg",rgb,Nx,Ny);
		
		pose_storage.clear();
		
		
		fprintf(stderr,"#################### SCAN ###############################\n");
		readMCLfile("scanMCL_pose.txt",pose_storage);
		
		cnt=0;
		while(cnt<pose_storage[0].size()){
			for(int j = 0; j<NUM_OF_MCL_RUNS;j++){
				ocoord.x = pose_storage[j][cnt].x;ocoord.y = pose_storage[j][cnt].y;
				pix = O->toPixel(ocoord);
					//setRGBpixBold(rgb,pix.x,pix.y, Nx, color_r[j],color_g[j], color_b[j] );
				setRGBpixBold(rgb,pix.x,pix.y, Nx, 0, 0,115+20*j );
			}
			cnt++;
			usleep(1000*1);
			if(cnt%20==0) displayImage(0, rgb,Ny,Nx);
		}
		
		
		pose_storage.clear();
		
		fprintf(stderr,"#################### TOPOSCAN ###############################\n");
		readMCLfile("toposcanMCL_pose.txt",pose_storage);
		
		cnt=0;
		while(cnt<pose_storage[0].size()){
			for(int j = 0; j<NUM_OF_MCL_RUNS;j++){
				ocoord.x = pose_storage[j][cnt].x;ocoord.y = pose_storage[j][cnt].y;
				pix = O->toPixel(ocoord);
					//setRGBpixBold(rgb,pix.x,pix.y, Nx, color_r[j],color_g[j], color_b[j] );
				setRGBpixBold(rgb,pix.x,pix.y, Nx, 115+20*j,0,0 );
			}
			cnt++;
			usleep(1000*1);
			if(cnt%20==0) displayImage(0, rgb,Ny,Nx);
		}
		saveJPEG("MCL_result.jpg",rgb,Nx,Ny);
		pose_storage.clear();
#endif
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		
		
		
		fprintf(stderr,"THE END\n");
		usleep(1000*5000);
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		
		
		return 0;
}		